import binascii
import os
import time
import socket
import sys
import threading
import struct, pickle
import cv2
import RPi.GPIO as GPIO
from _XiaoRGEEK_SERVO_ import XR_Servo

GPIO.setmode(GPIO.BCM)

##################### DO NOT CHANGE! (start) #####################

MAX_FREQUENCY = 1000

# LED
LED0 = 10
LED1 = 9
LED2 = 25
GPIO.setwarnings(False)
GPIO.setup(LED0, GPIO.OUT, initial=GPIO.HIGH)
GPIO.setup(LED1, GPIO.OUT, initial=GPIO.HIGH)
GPIO.setup(LED2, GPIO.OUT, initial=GPIO.HIGH)

# motors
ENA, A_speed = 13, 1
ENB, B_speed = 20, 1
IN1 = 19
IN2 = 16
IN3 = 21
IN4 = 26
GPIO.setup(ENA, GPIO.OUT, initial=GPIO.LOW)
ENA_pwm = GPIO.PWM(ENA, MAX_FREQUENCY)
ENA_pwm.start(0)
ENA_pwm.ChangeDutyCycle(100 * A_speed)
GPIO.setup(ENB, GPIO.OUT, initial=GPIO.LOW)
ENB_pwm = GPIO.PWM(ENB, MAX_FREQUENCY)
ENB_pwm.start(0)
ENB_pwm.ChangeDutyCycle(100 * B_speed)
GPIO.setup(IN1, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(IN2, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(IN3, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(IN4, GPIO.OUT, initial=GPIO.LOW)

# servo
Servo = XR_Servo()
class ServoInfo:
	def __init__(self, servo_id, servo_angle = -1):
		if servo_angle < 0:
			servo_angle = 90 if servo_id == 7 else 0
		self.id = servo_id
		self.angle = servo_angle
		Servo.XiaoRGEEK_SetServoAngle(self.id, self.angle)
	def turn(self, delta_angle):
		self.angle += delta_angle
		if self.angle > 180:
			self.angle = 180
		if self.angle < 0:
			self.angle = 0
		Servo.XiaoRGEEK_SetServoAngle(self.id, self.angle)
		return self.id, self.angle
	def setAngle(self, angle):
		self.angle = angle
		if self.angle > 180:
			self.angle = 180
		if self.angle < 0:
			self.angle = 0
		Servo.XiaoRGEEK_SetServoAngle(self.id, self.angle)
		return self.id, self.angle
horizontal_servo = ServoInfo(7)
vertical_servo = ServoInfo(8)
servos = { 7: horizontal_servo, 8: vertical_servo }

# supersonic
ECHO = 4
TRIG = 17
GPIO.setup(TRIG, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(ECHO, GPIO.IN, pull_up_down=GPIO.PUD_UP)

##################### DO NOT CHANGE! ( end ) #####################

def initLight(): # False: light on, True: light off
	GPIO.output(LED0, False)
	GPIO.output(LED1, False)
	GPIO.output(LED2, False)
	time.sleep(.5)
	
	GPIO.output(LED0, True)
	GPIO.output(LED1, False)
	GPIO.output(LED2, False)
	time.sleep(.5)
	
	GPIO.output(LED0, False)
	GPIO.output(LED1, True)
	GPIO.output(LED2, False)
	time.sleep(.5)
	
	GPIO.output(LED0, False)
	GPIO.output(LED1, False)
	GPIO.output(LED2, True)
	time.sleep(.5)
	
	GPIO.output(LED0, False)
	GPIO.output(LED1, False)
	GPIO.output(LED2, False)
	time.sleep(.5)
	
	GPIO.output(LED0, True)
	GPIO.output(LED1, True)
	GPIO.output(LED2, True)
	time.sleep(.5)
	
	return None

def setLeftMotorSpeed(speed):
	global A_speed
	A_speed = speed
	ENA_pwm.ChangeDutyCycle(speed * 100)
	return None
def setRightMotorSpeed(speed):
	global B_speed
	B_speed = speed
	ENB_pwm.ChangeDutyCycle(speed * 100)
	return None
def motorForward(debug = False):
	if debug: print('motor forward')
	GPIO.output(ENA, True)
	GPIO.output(ENB, True)
	GPIO.output(IN1, True)
	GPIO.output(IN2, False)
	GPIO.output(IN3, True)
	GPIO.output(IN4, False)
	return None
def motorBackward(debug = False):
	if debug: print('motor backward')
	GPIO.output(ENA, True)
	GPIO.output(ENB, True)
	GPIO.output(IN1, False)
	GPIO.output(IN2, True)
	GPIO.output(IN3, False)
	GPIO.output(IN4, True)
	return None
def motorLeft(debug = False):
	if debug: print('motor turn left')
	GPIO.output(ENA, True)
	GPIO.output(ENB, True)
	GPIO.output(IN1, True)
	GPIO.output(IN2, False)
	GPIO.output(IN3, False)
	GPIO.output(IN4, True)
	return None
def motorRight(debug = False):
	if debug: print('motor turn right')
	GPIO.output(ENA, True)
	GPIO.output(ENB, True)
	GPIO.output(IN1, False)
	GPIO.output(IN2, True)
	GPIO.output(IN3, True)
	GPIO.output(IN4, False)
	return None
def motorLeftForward(debug = False):
	if debug: print('motor left forward')
	GPIO.output(ENA, True)
	GPIO.output(ENB, True)
	GPIO.output(IN1, True)
	GPIO.output(IN2, False)
	GPIO.output(IN3, False)
	GPIO.output(IN4, False)
	return None
def motorLeftBackward(debug = False):
	if debug: print('motor left backward')
	GPIO.output(ENA, True)
	GPIO.output(ENB, True)
	GPIO.output(IN1, False)
	GPIO.output(IN2, True)
	GPIO.output(IN3, False)
	GPIO.output(IN4, False)
	return None
def motorRightForward(debug = False):
	if debug: print('motor right forward')
	GPIO.output(ENA, True)
	GPIO.output(ENB, True)
	GPIO.output(IN1, False)
	GPIO.output(IN2, False)
	GPIO.output(IN3, True)
	GPIO.output(IN4, False)
	return None
def motorRightBackward(debug = False):
	if debug: print('motor right backward')
	GPIO.output(ENA, True)
	GPIO.output(ENB, True)
	GPIO.output(IN1, False)
	GPIO.output(IN2, False)
	GPIO.output(IN3, False)
	GPIO.output(IN4, True)
	return None
def motorStop(debug = False):
	if debug: print('motor stop')
	GPIO.output(ENA, True)
	GPIO.output(ENB, True)
	GPIO.output(IN1, False)
	GPIO.output(IN2, False)
	GPIO.output(IN3, False)
	GPIO.output(IN4, False)
	return None

def setServoAngle(servo_id, servo_angle, delay = 0.1):
	servos[servo_id].setAngle(servo_angle)
	time.sleep(delay)
	return None
def servoTurnUp(angle = 10, delay = 0.05):
	vertical_servo.turn(angle)
	time.sleep(delay)
	return None
def servoTurnDown(angle = 10, delay = 0.05):
	vertical_servo.turn(-angle)
	time.sleep(delay)
	return None
def servoTurnLeft(angle = 10, delay = 0.05):
	horizontal_servo.turn(angle)
	time.sleep(delay)
	return None
def servoTurnRight(angle = 10, delay = 0.05):
	horizontal_servo.turn(-angle)
	time.sleep(delay)
	return None

def getDistance(): # unit: cm
	time.sleep(.01)
	GPIO.output(TRIG, GPIO.HIGH)
	time.sleep(.000015)
	GPIO.output(TRIG, GPIO.LOW)
	start_time = time.time()
	while not GPIO.input(ECHO):
		if time.time() - start_time > 6 / 340:
			break
		pass
	start_time = time.time()
	while GPIO.input(ECHO):
		pass
	end_time = time.time()
	res = (end_time - start_time) * 340 / 2 * 100
	time.sleep(.01)
	return res

setLeftMotorSpeed(1.)
setRightMotorSpeed(1.)

def execute(cmd):
	if len(cmd) != 3:
		return 0
	if cmd[0] == '00':
		if cmd[1] == '00': motorStop()
		if cmd[1] == '01': motorForward()
		if cmd[1] == '02': motorBackward()
		if cmd[1] == '03': motorLeft()
		if cmd[1] == '04': motorRight()
		if cmd[1] == '05': motorLeftForward()
		if cmd[1] == '06': motorLeftBackward()
		if cmd[1] == '07': motorRightForward()
		if cmd[1] == '08': motorRightBackward()
	if cmd[0] == '01':
		setServoAngle(int(cmd[1], 16), int(cmd[2], 16))
	if cmd[0] == 'fc':
		if cmd[1] == '00' and cmd[2] == '00': return 1
	return 0

try:
	this_server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
	this_server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
	this_server.bind(('', 2333))
	this_server.listen(1)
except socket.error as msg:
	print(msg)
	sys.exit(1)
# try:
# 	this_server_sonic = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
# 	this_server_sonic.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
# 	this_server_sonic.bind(('', 2334))
# 	this_server_sonic.listen(1)
# except socket.error as msg:
# 	print(msg)
# 	sys.exit(1)
# try:
# 	this_server_image = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
# 	this_server_image.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
# 	this_server_image.bind(('', 2335))
# 	this_server_image.listen(1)
# except socket.error as msg:
# 	print(msg)
# 	sys.exit(1)

def receiveCommand(conn):
	now_command, receiving = [], False
	while True:
		try:
			data = binascii.b2a_hex(conn.recv(1)).decode()
		except socket.error as msg:
			print('receiveCommand():', msg)
			break
		if data == 'ff':
			if receiving and len(now_command) > 0:
				receiving = False
				if execute(now_command): break
			else:
				receiving = True
				now_command = []
		else:
			if receiving: now_command.append(data)
	conn.close()
	return None

def sendMsgCharByChar(conn, msg, delay = .005):
	msg = '\xFF' + msg + '\xFF'
	for c in msg:
		conn.send(struct.pack('B', ord(c)))
		time.sleep(delay)
	return None

def sendCommand(conn): # 发送超声波信息
	while True:
		now_distance = min(255, int(getDistance()))
		print('now_distance:', now_distance)
		sendMsgCharByChar(conn, '\x03' + chr(now_distance) + chr(horizontal_servo.angle))
	return None

def sendImage(conn):
	my_camera = cv2.VideoCapture(0)
	while True:
		frame = my_camera.read()[1]
		frame = pickle.dumps(frame)
		conn.send(struct.pack('4si', 'SIZE'.encode(), len(frame)))
		print('send:', struct.pack('4si', 'SIZE'.encode(), len(frame)))
		recv_data = conn.recv(4).decode()
		print('recv:', recv_data)
		if recv_data == 'exit': break
		if recv_data != 'SIZE':
			print('Image fail. <1>')
			continue
		
		conn.sendall(frame)
		recv_data = conn.recv(4).decode()
		print('recv:', recv_data)
		if recv_data == 'exit': break
		if recv_data != 'PIC':
			print('Image fail. <2>')
			continue
		
		time.sleep(.02)
	my_camera.release()
	conn.close()

conn = this_server.accept()[0]
data = conn.recv(1024).decode()
assert data == 'COMMAND'
print('COMMAND connected!')
threading.Thread(target=receiveCommand, args=(conn,), name='receiveCommand').start()

# conn = this_server_sonic.accept()[0]
# data = conn.recv(1024).decode()
# assert data == 'GET_DISTANCE'
# print('GET_DISTANCE connected!')
threading.Thread(target=sendCommand, args=(conn,), name='sendCommand').start()

# conn = this_server_image.accept()[0]
# data = conn.recv(1024).decode()
# assert data == 'IMAGE'
# print('IMAGE connected!')
# threading.Thread(target=sendImage, args=(conn,), name='sendImage').start()

while len(threading.enumerate()) > 1:
	print(threading.enumerate())
	time.sleep(.5)
	pass

this_server.close()
# this_server_sonic.close()
# this_server_image.close()

print('end')